#include <chrono>
#include <thread>
#include <cstring>
#include <fstream>
#include "igslib_iutil.h"
#include "mh_mcu.h"



#define timout_s 0x80000 
#define SN_LEN 23

//#define _MY_DEBUG 
#ifdef _MY_DEBUG 
    //#define dbg_printf(fmt,x...) printf("%s,%d:"fmt,__FUNCTION__,__LINE__,##x)
    //#define dbg_printf(fmt,x...) printf(fmt,##x)
#else
    #define  dbg_printf(fmt,x...)
#endif

namespace igslib
{
    enum command{
        VERSION=0xc1,
        START=0xc2,
        ERASE=0xC3,
        LOAD=0xc4,
        PROG=0xc5,
        END=0xc6
    };

    enum respons{
        ACK=0xA3,
        NACK=0xA4
    };

    mh_mcu::mh_mcu()
    { 
        memset(Uart_TxBuf,0,sizeof(Uart_TxBuf)); 
        memset(Uart_RxBuf,0,sizeof(Uart_RxBuf));
    }
    mh_mcu::~mh_mcu()
    { 
        slient_delete(rawdata);
    }
    std::string mh_mcu::GetSourceVersion() 
    {				
        std::string rtn("ERROR");
        std::ifstream fp;
        char buf_seq[80] = {0};
        char buf_sn[80] = {0};
        std::string fwstr;

        if(rawdata)
        {

            memcpy(buf_seq,rawdata,3);
            fwstr=buf_seq;
            if(fwstr=="FW:")
            {
                memcpy(buf_sn,rawdata+3,20);
                rtn=buf_sn;
            }
            else
            {
                util::Log("ImgBurn is broken");				
            }
            return rtn; 		  

        }
        if(!filename.empty())
        {
            fp.open(filename, std::ios::in);
            if(!fp)
            {
                dbg_printf("open ImgBurn fail %s\n",filename);					
                std::stringstream ss;
                ss << __func__ << "> filename = " << filename;
                util::Log(ss.str().c_str());			
                return rtn;
            }
            else
            {
                dbg_printf("open ImgBurn success %s\n",filename);
            }
            fp.read(buf_seq,3);
            fwstr=buf_seq;
            if(fwstr=="FW:")
            {
                fp.read(buf_sn,20);
                rtn=buf_sn;
            }
            else
            {
                dbg_printf("buf_seq: %s\n",buf_seq);
                util::Log("ImgBurn is broken");				
            }			
            fp.close();			
            return rtn; 			
        }
        util::Log("MCU ImageBurn's Raw Data & filename is null");	
        return rtn;
    }

    void mh_mcu::ImportRawData(char* data,int len)
    {
        if(!rawdata)
        {
            rawdata=(char*)malloc(len);
            RawDlen=len;		
        }
        memcpy(rawdata,data,len);	
    }

    std::string mh_mcu::GetVersion()
    {
        std::string rtn;
        int	ret,length=0 ;
        int i=0,j=0;
        char CMD=VERSION;
        char data[20] = {0};
        char dtemp;
        unsigned int timeout_cnt=0;

        uart->Send(&CMD,1);

        while(length < 20)
        {
            ret=uart->Receive(&dtemp,1);
            if(ret>0)
            {
                if(dtemp!=0x00)
                    rtn+=dtemp;
                data[length++]=dtemp;
                dbg_printf("%c \n",dtemp);
            }

            if(timeout_cnt++>0x100000) //2s
            {	
                util::Log("Get MCU's revision which is time out");
                rtn = "ERROR";
                break;
            }
        }				
        return rtn;	
    }

    unsigned char mh_mcu::u8checksum(char *ptr,int len)
    {
        unsigned char cksum=0;

        for(int i=0;i<len;i++)
        {
            cksum+=(unsigned char)*(ptr+i);
        }
        return cksum;
    }

    int mh_mcu::wait_ack(int bs)
    {
        unsigned int timeout_cnt=0; 
        char dtemp=0;
        int ret=0;
        while(1)
        {
            dtemp=0;
            ret=uart->Receive(&dtemp,1);
            if(ret>0)
            {
                if(dtemp==ACK)
                {
                    return 0;
                }
                else if(dtemp==NACK)
                {
                    return -2;
                }
            }
            if(timeout_cnt++>bs*timout_s)
            {
                return -1;
            }
        }		
    }

    int mh_mcu::Upgrade()
    {
        int rtn = 0;
        //char filename_burn[]="/mnt/external_sd/MCU.bin";
        int	 i=0,j=0,ret,DL_loops=0,DL_remain=0,Dlen=0;
        char CMD=START;
        char checksum=0,Allchecksum=0;
        char buffer[1024] = {0};
        std::ifstream fp;			

        if(rawdata)
        {	
            /*Enter Bootload function*/	
            CMD=START;
            uart->Send(&CMD,1);
            util::SleepMSTime(3000); //sleep 3

            int flen = RawDlen;
            char* dptr=rawdata;
            dbg_printf("burn file size %d\n",flen);		
            Dlen=flen-SN_LEN; //Serial number ex. "FW:MA_AH000_V100XXXXXXXXX"
            dptr+=23;		

            /*ERASE function*/
            CMD=ERASE;
            uart->Send(&CMD,1);		
            ret=wait_ack(60);
            if(ret==-1)
            {
                dbg_printf("Erase time out  1min \n");					
                rtn = -1;
                util::Log("Erase time out ");		
                return rtn;
            }
            else if(ret==-2)
            {
                dbg_printf("Erase Fail\n");					
                util::Log("Erase Fail");		
                rtn = -2;
                return rtn;
            }
            else
            {
                dbg_printf("Erase success \n");
            }

            /*Data load & Program function*/					
            DL_loops=Dlen/1024;
            DL_remain=Dlen%1024;			

            for(int idx=0;idx<DL_loops+1;idx++)
            {
                if(idx!=DL_loops)
                {
                    memcpy(buffer,dptr,1024);
                    dptr+=1024;						
                    checksum=u8checksum(buffer,1024);			
                }
                else
                {
                    fp.read(buffer,DL_remain);
                    memcpy(buffer,dptr,DL_remain);
                    dptr+=DL_remain;	
                    for(i=DL_remain;i<1024;i++)
                        buffer[i]=0xFF;				
                }

                checksum=u8checksum(buffer,1024);
                Allchecksum+=checksum;
                CMD=LOAD;
                uart->Send(&CMD,1);	
                uart->Send(buffer,1024);	
                uart->Send(&checksum,1);

                ret=wait_ack(2);
                if(ret==-1)
                {
                    dbg_printf(" Loading Data time out  2s \n");					
                    util::Log("Loading Data time out");		
                    rtn = -3;
                    return rtn;
                }
                else if(ret==-2)
                {
                    dbg_printf("DL_loops:%d Loading Data checksum: %02x\n",idx,checksum);				
                    rtn = -4;
                    util::Log("Loading Data checksum Fail");		
                    return rtn;
                }
                else
                {
                    dbg_printf("DL_loops:%d Loading success checksum: %02x\n",idx,checksum);
                }

                CMD=PROG;
                uart->Send(&CMD,1);			
                ret=wait_ack(5);
                if(ret==-1)
                {
                    dbg_printf(" Program time out  5s \n");					
                    rtn = -5;
                    util::Log("Program time out");		
                    return rtn;
                }
                else if(ret==-2)
                {
                    dbg_printf("DL_loops:%d Program Fail Allchecksum: %02x\n",idx,Allchecksum);				
                    rtn = -6;
                    util::Log("Program Fail");		
                    return rtn;
                }
                else
                {
                    dbg_printf("DL_loops:%d Program success Allchecksum: %02x\n",idx,Allchecksum);	
                }
            }

            /*END function*/			
            CMD=END;
            uart->Send(&CMD,1);
            uart->Send(&Allchecksum,1);

            ret=wait_ack(2);
            if(ret==-1)
            {
                dbg_printf("END CMD time out 2s \n");					
                rtn = -7;
                util::Log("END CMD time out");		
                return rtn;
            }
            else if(ret==-2)
            {
                dbg_printf("END CMD Fail \n");				
                rtn = -8;
                util::Log("END CMD Fail");		
                return rtn;
            }
            else
            {
                dbg_printf("END  success checksum%02x\n",Allchecksum);
                std::this_thread::sleep_for(std::chrono::milliseconds(500));
            }	
            return rtn;
        }


        if(!filename.empty())
        {
            /*Enter Bootload function*/	
            CMD=START;
            uart->Send(&CMD,1);
            util::SleepMSTime(3000); //sleep 3

            fp.open(filename, std::ios::in | std::ios::binary | std::ios::ate );
            if(!fp)
            {
                dbg_printf("open ImgBurn fail %s\n",filename);					
                rtn = -9;
                util::Log("open ImgBurn fail");			
                return rtn;
            }
            else
            {
                dbg_printf("open ImgBurn success %s\n",filename);
            }

            // get length of file & valid data length:
            fp.seekg (0, fp.end);  //offset:0  start pos: file end
            int flen = fp.tellg(); //locate pos
            fp.seekg (0, fp.beg);
            dbg_printf("burn file size %d\n",flen);		

            Dlen=flen-SN_LEN; //Serial number ex. "FW:MA_AH000_V100XXXXXXXXX"
            fp.seekg (SN_LEN);


            /*ERASE function*/
            CMD=ERASE;
            uart->Send(&CMD,1);		
            ret=wait_ack(60);
            if(ret==-1)
            {
                dbg_printf("Erase time out  1min \n");					
                rtn = -1;
                util::Log("Erase time out ");		
                return rtn;
            }
            else if(ret==-2)
            {
                dbg_printf("Erase Fail\n");					
                util::Log("Erase Fail");		
                rtn = -2;
                return rtn;
            }
            else
            {
                dbg_printf("Erase success \n");
            }

            /*Data load & Program function*/					
            DL_loops=Dlen/1024;
            DL_remain=Dlen%1024;			

            for(int idx=0;idx<DL_loops+1;idx++)
            {
                if(idx!=DL_loops)
                {
                    fp.read(buffer,1024);				
                    checksum=u8checksum(buffer,1024);			
                }
                else
                {
                    fp.read(buffer,DL_remain);
                    for(i=DL_remain;i<1024;i++)
                        buffer[i]=0xFF;				
                }

                checksum=u8checksum(buffer,1024);
                Allchecksum+=checksum;
                CMD=LOAD;
                uart->Send(&CMD,1);	
                uart->Send(buffer,1024);	
                uart->Send(&checksum,1);

                ret=wait_ack(2);
                if(ret==-1)
                {
                    dbg_printf(" Loading Data time out  2s \n");					
                    util::Log("Loading Data time out");		
                    rtn = -3;
                    return rtn;
                }
                else if(ret==-2)
                {
                    dbg_printf("DL_loops:%d Loading Data checksum: %02x\n",idx,checksum);				
                    rtn = -4;
                    util::Log("Loading Data checksum Fail");		
                    return rtn;
                }
                else
                {
                    dbg_printf("DL_loops:%d Loading success checksum: %02x\n",idx,checksum);
                }

                CMD=PROG;
                uart->Send(&CMD,1);			
                ret=wait_ack(5);
                if(ret==-1)
                {
                    dbg_printf(" Program time out  5s \n");					
                    rtn = -5;
                    util::Log("Program time out");		
                    return rtn;
                }
                else if(ret==-2)
                {
                    dbg_printf("DL_loops:%d Program Fail Allchecksum: %02x\n",idx,Allchecksum);				
                    rtn = -6;
                    util::Log("Program Fail");		
                    return rtn;
                }
                else
                {
                    dbg_printf("DL_loops:%d Program success Allchecksum: %02x\n",idx,Allchecksum);	
                }
            }

            /*END function*/			
            CMD=END;
            uart->Send(&CMD,1);
            uart->Send(&Allchecksum,1);

            ret=wait_ack(2);
            if(ret==-1)
            {
                dbg_printf("END CMD time out 2s \n");					
                rtn = -7;
                util::Log("END CMD time out");		
                return rtn;
            }
            else if(ret==-2)
            {
                dbg_printf("END CMD Fail \n");				
                rtn = -8;
                util::Log("END CMD Fail");		
                return rtn;
            }
            else
            {
                dbg_printf("END  success checksum%02x\n",Allchecksum);
            }
            fp.close();
            return rtn;
        }
        rtn = -10;
        util::Log("MCU ImageBurn's RawData & filename is null");	
        return rtn;
    }

    /*******************
     ** IKV Function    ** 
     ********************/
    bool ikv::SendRequest(const std::string& request,const std::vector<char>& args)
    {
        unsigned char dtmp[100];
        unsigned short rand = 0x0;

        optiga_uid      chip_id;
        unsigned short  pid = 0, vid = 0;
        unsigned char  chip_odc[78] = {0};
        uint8_t         *byteptr;

        GetState();
        GetState();
        if(ikv_state!=State::Idle)
            return false;


        if(request=="deposit")
        {
            ROMaddr=args[0];
            return true;
        }
        else if(request=="withdraw")
        {  
            Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);			
            tmp->ikv_SetROMaddr(args[0]);
            ikv_state=State::WaitWithdraw;
            return true;
        }
        else if(request=="authenticate")
        {
            Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);			
            int rlen=tmp->ikv_GetInfo(dtmp);

            pid=(dtmp[1]<<8) |dtmp[0];
            vid=(dtmp[3]<<8) |dtmp[2];

            for(int i=0;i<10;i++)
                chip_id.bn[i] = dtmp[4+i];
            for(int i=0;i<78;i++)
                chip_odc[i] = dtmp[14+i];

            rand= util::GetTicks()&0xffff;
            /* 1. rand init */ 
            optiga_hst_rand_init(rand);  //產生32B  rand pool
            /* 2. session init with chip id */
            optiga_hst_session_init(&auth_session, &chip_id);
            /* 3. select public key for odc (certificate) verification with pid, vid (can be hardcoded 0x05, 0x05) */
            optiga_curve_init(pid, vid);


            /* 4. verify odc */
            if (optiga_hst_verify_odc (&auth_session, chip_odc) == FALSE) {
                //printf("[ERROR] ODC Verification Failed. \n");
                return 0;
            } else {
                //printf("[DEBUG] ODC Verification OK. \n");
            }

            /* 5. generate challenge */
            optiga_hst_gen_challenge (&auth_session);
            byteptr = (uint8_t *)auth_session.lambda;
            // printf("lambda    = [");
            // for (int i = 0; i < sizeof(auth_session.lambda); i++) {
            // printf("%02X ", byteptr[i]);
            // }
            // printf("]\n");
            byteptr = (uint8_t *)auth_session.challenge;
            // printf("challenge = [");
            // for (int i = 0; i < sizeof(auth_session.challenge); i++) {
            // printf("%02X ", byteptr[i]);
            // }
            // printf("]\n");  	  
            tmp->ikv_SetChallenge(byteptr);

            ikv_state=State::WaitAuthentication;
            return true;
        }
        return false;

    }
    std::vector<char> ikv::Withdraw()
    {
        std::vector<char> rtn;
        unsigned char cptr[16];

        Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);
        tmp->ikv_GetROMData(cptr);
        for(int i=0;i<16;i++)
            rtn.push_back((char)cptr[i]);		
        return rtn;
    }
    void ikv::Deposit(const std::vector<char>& data)
    {
        char cptr[16];
        int len=data.size();
        if(len<16)				
        {
            memcpy(cptr,&data,len);	
            for(int i=0;i<(16-len);i++)
                cptr[len+i]=0;
        }
        else
            memcpy(cptr,&data[0],16);

        Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);
        tmp->ikv_SetROMData(ROMaddr,cptr);
        ikv_state=State::WaitWithdraw;
    }
    bool ikv::Authenticate()
    {  
        bool rtn = false;
        unsigned char dtmp[32],response[22],mac_data[10];
        Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);
        tmp->ikv_GetResponse(dtmp);

        //util::Log("%s> response= ",__func__);
        for(int i=0;i<22;i++)
        {
            response[i]=dtmp[i];
            //util::Log("%02x ",response[i]);
        }
        //util::Log("\n");

        //util::Log("%s> mac_data= ",__func__);
        for(int i=0;i<10;i++)
        {
            mac_data[i]=dtmp[i+22];
            //util::Log("%02x ",mac_data[i]);
        }
        //util::Log("\n");

        //util::Log("%s> optiga_hst_verify_response = ",__func__);

        /* 6. verify response */
        if (optiga_hst_verify_response (&auth_session, response, mac_data) == FALSE){
            //printf("[ERROR] Response Verification Failed. \n");
            rtn= false;
        } else { 
            //printf("[DEBUG] Response Verification OK. \n");
            rtn= true;
        }		
        //util::Log("%d\n",rtn);
        return rtn;
    }
    IProtectUnit::State ikv::GetState()
    {
        Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi); 
        //util::Log("%s#1> ikv_busy=%d,ikv_state=%d\n",__func__,tmp->ikv_IsBusy(),ikv_state);
        if(tmp->ikv_IsBusy()==true)
        {       
            return ikv_state;
        }
        else
        {
            ikv_state=State::Idle;
            //util::Log("%s#2> ikv_busy=%d,ikv_state=%d\n",__func__,tmp->ikv_IsBusy(),ikv_state);
            return ikv_state;
        }
    }
    void ikv::ProtectEnable()
    {
        Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);
        tmp->ikv_Setup();
        ikv_state=State::WaitIKVWork;
    }	
    void ikv::Seal()
    {
        Protocol_DragonBall* tmp = (Protocol_DragonBall*)(m01spi);
        tmp->ikv_DLock();
    }
};

